
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_view.h
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct {
    int32_t roll_sensor_cd;
    int32_t pitch_sensor_cd;
    int32_t yaw_sensor_cd;

    // 单位：rad/s
    float roll;
    float pitch;
    float yaw;

    float cos_roll;
    float cos_pitch;
    float cos_yaw;
    float sin_roll;
    float sin_pitch;
    float sin_yaw;

    Quat_t attitude_body;

    Vector3f_t gyr;
    Vector3f_t acc;
    Vector3f_t accel_ef;    // 导航坐标系下的三轴加速度，包含重力,m/s/s

    Vector3f_t relpos_cm;   // NEU
    Vector3f_t velocity_cm; // NEU

    Location curr_loc;
} ahrs_view;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void ahrs_view_update(ahrs_view *ahrs);

static inline  Vector3f_t get_accel_ef_blended(ahrs_view *ahrs) { return ahrs->accel_ef;}

/**
 * get_position - returns the current position relative to the home location in cm.
 *
 * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
 *
 * @return
 */
static inline Vector3f_t     ahrs_get_position(ahrs_view *ahrs) {return ahrs->relpos_cm;}

/**
 * get_velocity - returns the current velocity in cm/s
 *
 * @return velocity vector:
 *              .x : latitude  velocity in cm/s
 *              .y : longitude velocity in cm/s
 *              .z : vertical  velocity in cm/s
 */
static inline  Vector3f_t    ahrs_get_velocity(ahrs_view *ahrs) {return ahrs->velocity_cm;}

/**
 * get_speed_xy - returns the current horizontal speed in cm/s
 *
 * @returns the current horizontal speed in cm/s
 */
static inline float          ahrs_get_speed_xy(ahrs_view *ahrs) {return math_norm(ahrs->velocity_cm.x, ahrs->velocity_cm.y);}

/**
 * get_altitude - get latest altitude estimate in cm
 * @return
 */
static inline float          ahrs_get_altitude(ahrs_view *ahrs) {return ahrs->relpos_cm.z;}

/**
 * get_velocity_z - returns the current climbrate.
 *
 * @see get_velocity().z
 *
 * @return climbrate in cm/s
 */
static inline float          ahrs_get_velocity_z(ahrs_view *ahrs) {return ahrs->velocity_cm.z;}

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



